package test.crane;

import ptolemy.actor.*;
import ptolemy.kernel.*;
import ptolemy.kernel.util.*;
import ptolemy.data.*;
import ptolemy.data.type.*;

//////////////////////////////////////////////////////////////////////////
//// ControlAlg
/**

@author Ivan Jeukens
@version v 0.1 07/02/2002
*/
public class ControlAlg extends TypedAtomicActor {

    public ControlAlg(CompositeEntity container, String name) throws
            IllegalActionException, NameDuplicationException {
        super(container, name);

        shutdown = new TypedIOPort(this, "shutdown", true, false);
        shutdown.setTypeEquals(BaseType.GENERAL);

        trigger = new TypedIOPort(this, "trigger", true, false);
        trigger.setTypeEquals(BaseType.GENERAL);

        emmode = new TypedIOPort(this, "emmode", true, false);
        emmode.setTypeEquals(BaseType.GENERAL);

        emstop = new TypedIOPort(this, "emstop", true, false);
        emstop.setTypeEquals(BaseType.GENERAL);

        posdesired = new TypedIOPort(this, "posdesired", true, false);
        posdesired.setTypeEquals(BaseType.DOUBLE);

        alfa = new TypedIOPort(this, "alfa", true, false);
        alfa.setTypeEquals(BaseType.DOUBLE);

        poscar =new TypedIOPort(this, "poscar", true, false);
        poscar.setTypeEquals(BaseType.DOUBLE);

        vc = new TypedIOPort(this, "vc", false, true);
        vc.setTypeEquals(BaseType.DOUBLE);
        
        brake = new TypedIOPort(this, "brake", true, false);
        brake.setTypeEquals(BaseType.BOOLEAN);

        _q = new double[5];

        new Attribute(this, "_nonStrictMarker");
    }

    public TypedIOPort trigger;
    public TypedIOPort shutdown;
    
    public TypedIOPort emmode;
    public TypedIOPort posdesired;
    public TypedIOPort emstop;
    public TypedIOPort alfa;
    public TypedIOPort poscar;
    public TypedIOPort brake;
    
    public TypedIOPort vc;

    ///////////////////////////////////////////////////////////////////
    ////                         public methods                    ////

    public void pruneDependencies() {
        super.pruneDependencies();
        removeDependency(trigger, vc);
        removeDependency(brake, vc);
    }
    
    public void initialize() throws IllegalActionException  {
        super.initialize();
        _reset();
    }

    public void fire() throws IllegalActionException {
        if(emmode.isKnown()) {
            if(emmode.hasToken(0)) {
                emmode.get(0);
                _inEmMode = true;
            }
        }

        if(posdesired.isKnown()) {
            if(posdesired.hasToken(0)) {
                _posdesired = ((DoubleToken)posdesired.get(0)).doubleValue();
            }
        }

        if(_state == ZEROVC) {
            vc.broadcast(new DoubleToken(0.0));
        }
        else
        if(_state == RUNNING) {
            if(poscar.isKnown() && alfa.isKnown() && emstop.isKnown() &&
                   shutdown.isKnown() && !vc.isKnown()) {
            
            //if(poscar.isKnown() && alfa.isKnown() && emstop.isKnown() &&
            //    shutdown.isKnown()) {

                 if(emstop.hasToken(0)) {
                     vc.broadcast(new DoubleToken(0.0));
                 }
                 else
                 if(shutdown.hasToken(0)) {
                     vc.broadcast(new DoubleToken(0.0));
                 }
                 else
                 if(poscar.hasToken(0)) {
                    _poscar = ((DoubleToken)poscar.get(0)).doubleValue();
                    double _alfa = ((DoubleToken)alfa.get(0)).doubleValue();

                    _count++;
                    if(_count == 5) {
                        _count = 0;

                        double y = 0.0;
                        for(int i = 0;i < 5;i++) {
                            y = y + _K[i] * _q[i];
                        }

                        //System.out.println("y = " + y);

                        double z;
                        if(_inEmMode) {
                            z = _q[1];
                        }
                        else {
                            z = _alfa;
                        }
                        //System.out.println("Z = " + z);

                        z = _poscar + 0.5 * z;

                        //System.out.println("Z' = " + z);

                        double u = _kp * (_posdesired - z);

                        // System.out.println("u = " + u + " y = " + y);
                        double v;
                        u = u - y;

                        if(u > 40.0) {
                            v = 40.0;
                        }
                        else
                        if(u < -40.0) {
                            v = -40.0;
                        }
                        else {
                            v = u;
                        }
                        //System.out.println("V = " + v);
                        vc.broadcast(new DoubleToken(v));

                        double [] qnext = new double[5];
                        for(int i = 0;i < 5;i++) {
                            qnext[i] = _B[i][0] * v + _B[i][1] * _poscar;
                            for(int j = 0;j < 5;j++) {
                                qnext[i] = qnext[i] + _A[i][j] * _q[j];
                            }
                        }
                        for(int i = 0;i < 5;i++) _q[i] = qnext[i];
                    }
                }
            }
        }

        if(poscar.isKnown() && alfa.isKnown() && emstop.isKnown()
                && shutdown.isKnown()  && !vc.isKnown()) {
            vc.broadcastClear();
        }
    }

    public boolean postfire() throws IllegalActionException {

        switch(_state) {
            case READY: {
                if(trigger.isKnown()) {
                    if(trigger.hasToken(0)) {
                        trigger.get(0);
                        if(posdesired.isKnown() && posdesired.hasToken(0)) {
                            _state = RUNNING;
                            System.out.println("Control alg running");
                        }
                        else {
                            _state = WAITING;
                            System.out.println("Control alg waiting");
                        }
                    }
                }
            } break;

            case WAITING: {
                if(shutdown.isKnown()) {
                    if(shutdown.hasToken(0)) {
                        shutdown.get(0);
                        _reset();
                        break;
                    }
                }

                if(emstop.isKnown()) {
                    if(emstop.hasToken(0)) {
                        emstop.get(0);
                        _reset();
                        break;
                    }
                }

                if(posdesired.isKnown()) {
                    if(posdesired.hasToken(0)) {
                        _state = RUNNING;
                        System.out.println("Control alg running");
                    }
                }
            } break;
            
            case RUNNING: {
                if(shutdown.isKnown()) {
                    if(shutdown.hasToken(0)) {
                        shutdown.get(0);
                        _reset();
                        break;
                    }
                }
                
                if(emstop.isKnown()) {
                    if(emstop.hasToken(0)) {
                        emstop.get(0);
                        _reset();
                        break;
                    }
                }

                if(brake.isKnown()) {
                    if(brake.hasToken(0) && !emstop.hasToken(0)) {
                        if(((BooleanToken) brake.get(0)).booleanValue()) {
                            _posdesired = -10.0;
                            _count = 0;
                            _state = ZEROVC;
                            System.out.println("Control alg braking");
                        }
                    }
                }
            } break;
            
            case ZEROVC : {
                if(shutdown.isKnown()) {
                    if(shutdown.hasToken(0)) {
                        shutdown.get(0);
                        _reset();
                        break;
                    }
                }
                
                if(emstop.isKnown()) {
                    if(emstop.hasToken(0)) {
                        emstop.get(0);
                        _reset();
                        break;
                    }
                }
                
                if(posdesired.isKnown()) {
                    if(posdesired.hasToken(0)) {
                        _state = RUNNING;
                        System.out.println("Control alg running");
                    }
                    else {
                        _state = WAITING;
                        System.out.println("Control alg waiting");
                    }
                }
            } break;
        }

        return true;
    }

    ///////////////////////////////////////////////////////////////////
    ////                        private methods                    ////

    private void _reset() {
        _state = READY;
        _inEmMode = false;
        _count = 0;
        _posdesired = -10.0;
        for(int i = 0;i < 5;i++) {
            _q[i] = 0.0;
        }
    }

    ///////////////////////////////////////////////////////////////////
    ////                      private variables                    ////

    private int _state;

    private boolean _inEmMode;    
    private double _posdesired;
    private double _poscar;
    private int _count;

    private double [] _q;

    private static final double [] _K = { +1.0906e+03, -3.2981e+03, 
        +5.7106e+00, +0.0000e+00, +3.0647e+03 }; 
    
    private static final double [][] _A = { 
        { +9.9950e-01, +9.8040e-01, +9.9444e-04, -2.9370e+01, +4.9032e-03 },
        { +4.9882e-07, +9.9892e-01, -9.9632e-07, +4.8569e+01, +9.9963e-03 },
        { +0.0000e+00, +0.0000e+00, +9.9004e-01, -5.2731e+04, +0.0000e+00 },
        { +9.9975e-03, +4.9033e-03, +4.9817e-06, +9.9757e-02, +1.6346e-05 },
        { +9.9739e-05, -2.1569e-01, -1.9888e-04, -9.5679e+01, +9.9892e-01} 
    };
        
    private static final double [][] _B = {
        { +1.9926e-05, +2.9370e+01 },
        { -1.3296e-08, -4.8569e+01 },
        { +3.9800e-02, +5.2731e+04 },
        { +6.6485e-08, +9.0024e-01 },
        { -3.9853e-06, +9.5679e+01 }
    };

    private static final double _kp = +5.5000e+02;


 /*     private  double [] _K; 
       private  double [][] _A;
      private  double [][] _B;
      private double _kp;
*/

    private static final int READY = 1;
    private static final int RUNNING = 2;
    private static final int WAITING = 3;
    private static final int ZEROVC= 4;
}
